/*
 * Copyright 1997   BG Systems, Inc.
 * encoder.c
 *
 * Routine to send and receive encoder data
 *
 * Author         Date       Comments
 * John Green     20-Nov-97  Written
 * John Green     29-Dec-97  Add support for incremental encoders
 */


#include <stdio.h>
#include <stdlib.h>

#include "lv3.h"
static char SccsId[] = "@(#)encoder.c	1.2 28 Dec 1997";

int pack_encoders(bglv *, char *, int *);
int unpack_encoders(bglv *, char *, int );

int pack_encoders(bglv *bgp, char *out_buf, int *k)
{
/*
 *  Not used at this time
 */
   return(0);
}

int unpack_encoders(bglv *bgp, char *buf, int k)
{
   int i;
   int enc0;
   int enc1;
   int enc2;
   int enc3;
   long increment;

   for ( i = 0; i < 8; i++ )
   {
/*
 *  First check to see if encoder is selected, and
 *  then check to see if it is set to be incremental
 * 
 *  The 24 bit encoder values come in 4 characters, each 
 *  containing 6 bits.  
 *  
 *  Each incoming character - buf[i] - is masked with 0x3f
 *  and assigned to a temporary variable enc0 etc.  This is
 *  done for clarity.
 *
 *  For absolute encoders, the values are shifted and
 *  then assigned to the data structure.
 *
 *  For incremental encoders, they are loaded into a temporary
 *  variable - increment.  Then a test is made to see if the
 *  increment is negative, in which case the difference between
 *  the encoder value and 2**24 is loaded as an _int_ to the
 *  data structure.  If the value is positive it is simply cast as
 *  an int and loaded.
 */
      if ( bgp->enc_sel & (0x01 << i) &&
           bgp->enc_type & (0x01 << i) )
      {
         enc0 = (buf[k]-0x21) & 0x3f;
         increment = enc0;
         enc1 = (buf[k+1]-0x21) & 0x3f;
         increment |= enc1 << 6;
         enc2 = (buf[k+2]-0x21) & 0x3f;
         increment |= enc2 << 12;
         enc3 = (buf[k+3]-0x21) & 0x3f;
         increment |= enc3 << 18;
         if ( increment & 0x100000 )
            bgp->enc_inc_val[i] = (int) (increment - 16777215);
         else
            bgp->enc_inc_val[i] = (int) increment;
         k += 4;

      }
      else if ( bgp->enc_sel & (0x01 << i) )
      {
         enc0 = (buf[k]-0x21) & 0x3f;
         bgp->enc_abs_val[i] = enc0;
         enc1 = (buf[k+1]-0x21) & 0x3f;
         bgp->enc_abs_val[i] |= enc1 << 6;
         enc2 = (buf[k+2]-0x21) & 0x3f;
         bgp->enc_abs_val[i] |= enc2 << 12;
         enc3 = (buf[k+3]-0x21) & 0x3f;
         bgp->enc_abs_val[i] |= enc3 << 18;
         k += 4;
      }
   }
   return(i);
}

